#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from std_msgs.msg import String, Int32
import cv2
import mediapipe as mp
import math
import json
from unitree_api.msg import Request  # 这是Request消息类型

class GestureRecognizer(Node):
    def __init__(self):
        super().__init__('gesture_recognizer')
        
        # 初始化MediaPipe
        self.mp_hands = mp.solutions.hands
        self.hands = self.mp_hands.Hands(
            static_image_mode=False,
            max_num_hands=1,
            min_detection_confidence=0.75,
            min_tracking_confidence=0.75)
        
        # AGIROS设置
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.image_callback,
            10)
        self.gesture_pub = self.create_publisher(Int32, '/hand_gesture', 10)
        self.debug_pub = self.create_publisher(String, '/gesture_debug', 10)
        
        # 添加运动请求发布器
        self.pub = self.create_publisher(Request, "/api/sport/request", 10)
        
        # 添加状态变量，防止重复发布
        self.last_gesture = None
        self.gesture_cooldown = 10  # 防止连续发布的冷却帧数
        self.cooldown_counter = 0
        
        self.get_logger().info("手势识别节点已启动，使用角度检测方法")

    def vector_2d_angle(self, v1, v2):
        '''求解二维向量的角度'''
        v1_x = v1[0]
        v1_y = v1[1]
        v2_x = v2[0]
        v2_y = v2[1]
        try:
            angle_ = math.degrees(math.acos((v1_x*v2_x+v1_y*v2_y)/(((v1_x**2+v1_y**2)**0.5)*((v2_x**2+v2_y**2)**0.5))))
        except:
            angle_ = 65535.
        if angle_ > 180.:
            angle_ = 65535.
        return angle_

    def hand_angle(self, hand_):
        '''获取对应手相关向量的二维角度,根据角度确定手势'''
        angle_list = []
        #---------------------------- thumb 大拇指角度
        angle_ = self.vector_2d_angle(
            ((int(hand_[0][0])- int(hand_[2][0])),(int(hand_[0][1])-int(hand_[2][1]))),
            ((int(hand_[3][0])- int(hand_[4][0])),(int(hand_[3][1])- int(hand_[4][1])))
        )
        angle_list.append(angle_)
        #---------------------------- index 食指角度
        angle_ = self.vector_2d_angle(
            ((int(hand_[0][0])-int(hand_[6][0])),(int(hand_[0][1])- int(hand_[6][1]))),
            ((int(hand_[7][0])- int(hand_[8][0])),(int(hand_[7][1])- int(hand_[8][1])))
        )
        angle_list.append(angle_)
        #---------------------------- middle 中指角度
        angle_ = self.vector_2d_angle(
            ((int(hand_[0][0])- int(hand_[10][0])),(int(hand_[0][1])- int(hand_[10][1]))),
            ((int(hand_[11][0])- int(hand_[12][0])),(int(hand_[11][1])- int(hand_[12][1])))
        )
        angle_list.append(angle_)
        #---------------------------- ring 无名指角度
        angle_ = self.vector_2d_angle(
            ((int(hand_[0][0])- int(hand_[14][0])),(int(hand_[0][1])- int(hand_[14][1]))),
            ((int(hand_[15][0])- int(hand_[16][0])),(int(hand_[15][1])- int(hand_[16][1])))
        )
        angle_list.append(angle_)
        #---------------------------- pink 小拇指角度
        angle_ = self.vector_2d_angle(
            ((int(hand_[0][0])- int(hand_[18][0])),(int(hand_[0][1])- int(hand_[18][1]))),
            ((int(hand_[19][0])- int(hand_[20][0])),(int(hand_[19][1])- int(hand_[20][1])))
        )
        angle_list.append(angle_)
        return angle_list

    def h_gesture(self, angle_list):
        '''二维约束的方法定义手势'''
        thr_angle = 65.
        thr_angle_thumb = 53.
        thr_angle_s = 49.
        gesture_str = None
        
        if 65535. not in angle_list:
            if (angle_list[0]>thr_angle_thumb) and (angle_list[1]>thr_angle) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]>thr_angle):
                gesture_str = 0  # fist
            elif (angle_list[0]<thr_angle_s) and (angle_list[1]<thr_angle_s) and (angle_list[2]<thr_angle_s) and (angle_list[3]<thr_angle_s) and (angle_list[4]<thr_angle_s):
                gesture_str = 5  # five
            elif (angle_list[0]<thr_angle_s)  and (angle_list[1]<thr_angle_s) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]>thr_angle):
                gesture_str = 7  # gun?
            elif (angle_list[0]<thr_angle_s)  and (angle_list[1]<thr_angle_s) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]<thr_angle_s):
                gesture_str = 8  # love?
            elif (angle_list[0]>5)  and (angle_list[1]<thr_angle_s) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]>thr_angle):
                gesture_str = 1  # one
            elif (angle_list[0]<thr_angle_s)  and (angle_list[1]>thr_angle) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]<thr_angle_s):
                gesture_str = 6  # six
            elif (angle_list[0]>thr_angle_thumb)  and (angle_list[1]<thr_angle_s) and (angle_list[2]<thr_angle_s) and (angle_list[3]<thr_angle_s) and (angle_list[4]>thr_angle):
                gesture_str = 3  # three
            elif (angle_list[0]<thr_angle_s)  and (angle_list[1]>thr_angle) and (angle_list[2]>thr_angle) and (angle_list[3]>thr_angle) and (angle_list[4]>thr_angle):
                gesture_str = 4  # four?
            elif (angle_list[0]>thr_angle_thumb)  and (angle_list[1]<thr_angle_s) and (angle_list[2]<thr_angle_s) and (angle_list[3]>thr_angle) and (angle_list[4]>thr_angle):
                gesture_str = 2  # two
        
        return gesture_str

    def publish(self, api_id, x=0.0, y=0.0, z=0.0):
        """通用发布方法"""
        req = Request()
        req.header.identity.api_id = api_id
        req.parameter = json.dumps({"x": x, "y": y, "z": z})
        self.pub.publish(req)
        self.get_logger().info(f"已发送指令: API={api_id}, x={x}, y={y}, z={z}")

    def image_callback(self, msg):
        try:
            # 转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
            
            # 水平翻转图像（镜像处理）
            rgb_image = cv2.flip(rgb_image, 1)
            
            # MediaPipe处理
            results = self.hands.process(rgb_image)
            
            gesture_number = -1  # -1表示未检测到手势
            
            if results.multi_hand_landmarks:
                for hand_landmarks in results.multi_hand_landmarks:
                    # 获取手部关键点坐标
                    hand_local = []
                    for i in range(21):
                        x = hand_landmarks.landmark[i].x * cv_image.shape[1]
                        y = hand_landmarks.landmark[i].y * cv_image.shape[0]
                        hand_local.append((x, y))
                    
                    if hand_local:
                        # 计算角度并识别手势
                        angle_list = self.hand_angle(hand_local)
                        gesture_number = self.h_gesture(angle_list)
                        
                        # 发布调试信息
                        debug_msg = String()
                        debug_msg.data = f"Angles: {[round(a, 1) for a in angle_list]}, Gesture: {gesture_number}"
                        self.debug_pub.publish(debug_msg)
                        
                        # 在图像上绘制关键点和手势
                        mp.solutions.drawing_utils.draw_landmarks(
                            cv_image, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
                        
                        # 显示手势数字
                        cv2.putText(cv_image, f"Gesture: {gesture_number}", (20, 50), 
                                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            
            # 发布手势数字
            gesture_msg = Int32()
            gesture_msg.data = gesture_number if gesture_number is not None else -1
            self.gesture_pub.publish(gesture_msg)
            
            # 检查是否需要发布运动指令
            if self.cooldown_counter > 0:
                self.cooldown_counter -= 1
                
            if gesture_number is not None and gesture_number != self.last_gesture and self.cooldown_counter == 0:
                if gesture_number == 2:  # 数字2手势
                    self.publish(1009)  # 坐下动作
                    self.cooldown_counter = self.gesture_cooldown
                    self.get_logger().info("检测到数字2手势，发布坐下指令")
                elif gesture_number == 3:  # 数字3手势
                    self.publish(1010)  # 站立动作
                    self.cooldown_counter = self.gesture_cooldown
                    self.get_logger().info("检测到数字3手势，发布站立指令")
                    
                self.last_gesture = gesture_number
            
            # 显示结果图像
            cv2.imshow('Hand Gesture Recognition', cv_image)
            cv2.waitKey(1)
            
        except Exception as e:
            self.get_logger().error(f"处理图像时出错: {str(e)}")

def main(args=None):
    rclpy.init(args=args)
    node = GestureRecognizer()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
        cv2.destroyAllWindows()

if __name__ == '__main__':
    main()